package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.CounterBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.commands.*;

/**
 *
 * @author Super NURDs
 */

public class DriveTrain extends Subsystem {

    public static final double PI = 3.14159265358979323846264338327950288;

    // The motor drives a 12 tooth sprocket
    // This is chained to 24 tooth sprocket that drives the wheel
    // Therefore, for every one rotation of the motor, the wheel rotates .5
    // Also for everyone one roration of the motor, the encoder rotates 3 times
    // Therefore, for every 3 encoder rotations, the wheel rotates 0.5
    // The wheel is a 6 in diameter
    // Therefore, distance travelled/wheel rot:
    //      (6 in * PI)
    //      -----------
    //      wheel rot
    // To compute ft / encoder pulse:
    //
    // (6 * PI) in   .5 wheel rot   1 enc rot    1 ft
    // ----------- * ------------ * ---------- * -----
    // 1 wheel rot     3 enc rot    250 pulses   12 in
    //
    // Result = .0010471976
    public static final double ENCODER_FEET_PER_PULSE = (6.0*PI) * (0.5/3.0) * (1.0 / 250.0) * (1.0/12.0);
    
    public boolean isHighGear;
    
    RobotDrive drive = null; 
    
    ShifterGearbox leftGearbox = null;
    ShifterGearbox rightGearbox = null;

    public double currentLeftSpeed = 0.0; 
    public double currentRightSpeed = 0.0;
    
    public Gyro gyroscope = null;
    public Encoder encoder = null;

    public DriveTrain() {
       super("DriveTrain");
         
       try {
           leftGearbox = new ShifterGearbox(RobotMap.TOP_LEFT_TALON, RobotMap.BOTTOM_LEFT_TALON,
                   RobotMap.SHIFTER_OPEN_SOLENOID, RobotMap.SHIFTER_CLOSE_SOLENOID);
           rightGearbox = new ShifterGearbox(RobotMap.TOP_RIGHT_TALON, RobotMap.BOTTOM_RIGHT_TALON,
                   RobotMap.SHIFTER_OPEN_SOLENOID, RobotMap.SHIFTER_CLOSE_SOLENOID);
           
           drive = new RobotDrive(leftGearbox.getBottomMotor(),leftGearbox.getTopMotor(),
                   rightGearbox.getBottomMotor(),rightGearbox.getTopMotor());
           drive.setSafetyEnabled(false);

           gyroscope = new Gyro(RobotMap.DRIVETRAIN_GYRO);

           encoder = new Encoder(RobotMap.RIGHT_DRIVE_ENCODER_CH_A,
                    RobotMap.RIGHT_DRIVE_ENCODER_CH_B, true, CounterBase.EncodingType.k2X);

           encoder.setDistancePerPulse(ENCODER_FEET_PER_PULSE);
       }
       catch (Exception e){
           e.printStackTrace();
        }  
   }
  
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    
    public void setSpeed(double s){
       this.setSpeed(s, s);
    }
    
    public void setSpeed(double leftSpeed, double rightSpeed){
        this.tankDrive(leftSpeed, rightSpeed);
    }
   
   public void stop() {
       this.setSpeed(0.0);
   }

    public void initDefaultCommand() {
       this.setDefaultCommand(new DoDriveTank());
    }
    
    public void tankDrive(double leftValue, double rightValue){
        try {
            currentLeftSpeed = leftValue;
            currentRightSpeed = rightValue;
            drive.tankDrive(leftValue, rightValue);
        }
        catch(Exception e) {
        }
    }
    
    public double currentLeftSpeed() {
        return currentLeftSpeed;
    }
    
    public double currentRightSpeed() {
        return currentRightSpeed;
    }
    
    public void lowGear(){
        try {
            leftGearbox.lowGear();
            rightGearbox.lowGear();
            isHighGear = false;
        }
        catch(Exception e) {
        }
    }
    
    public void highGear(){
        try {
            leftGearbox.highGear();
            rightGearbox.highGear();
            isHighGear = true;
        }
        catch(Exception e) {
        }
    }
        
    public boolean isHighGear() {
        return isHighGear;
    }
    
    public void resetGyro() {
        try {
            gyroscope.reset();
        }
        catch(Exception e) {
        }
    }
    
    public double getGyroAngle() {
        if(gyroscope == null) {
            return 0.0;
        }
        return gyroscope.getAngle();
    }
    
    public double getEncoderValue() {
        if(encoder == null) {
            return(0.0);
        }
        double encValue = -encoder.getDistance();
        return(encValue);
    }
    
    public void startEncoders() {
        try {
            encoder.start();
        }
        catch(Exception e) {
        }
    }

    public void stopEncoders() {
        try {
            encoder.stop();
        }
        catch(Exception e) {
        }
    }

    public void resetEncoders() {
        try {
            encoder.reset();
        }
        catch(Exception e) {
        }
    }
}